                                                                 /*云台电机*/
																																 
#include "stm32f4xx_hal.h"
#include "motorhead.h"
#include "can.h"
#include "sys.h"
#include "config.h"
Encoder mechanical_angle_PTZ_P;
Encoder mechanical_angle_PTZ_Y;
volatile Encoder Dial_the_motor;

int Dial_motor_speed_ref = 0;
int Dial_motor_speed_fdb = 0;

int32_t position_hit = 0;//拨弹电机位置
int speed_hit = 0;//拨弹电机转速
u8 prepare_flag=0;

void GetEncoderBias_Y(volatile Encoder *v)
{

	v->ecd_bias = YAW_INITIAL_VALUE;/*YAW电机初值*/
	v->ecd_value = v->ecd_bias;
	v->raw_value=v->ecd_bias;
	v->last_raw_value = v->ecd_bias;
}

void GetEncoderBias_P(volatile Encoder *v)
{

	v->ecd_bias = PITCH_INITIAL_VALUE;/*Pitch电机初值*/ 
	v->ecd_value = v->ecd_bias;
	v->raw_value=v->ecd_bias;
	v->last_raw_value = v->ecd_bias;  
           
}

void GetEncoderBias(volatile Encoder *v)
{

	v->ecd_bias = (can_1_buf[0]<<8)|can_1_buf[1];  /*保存初始编码器值作为偏差*/ 
	v->ecd_value = v->ecd_bias;
	v->last_raw_value = v->ecd_bias;
           
}

/*以下程序是把反馈量是编码值的电机转化为速度，需要用的电机有6623,6050*/
void EncoderProcess(volatile Encoder *v)
{
   
	v->last_raw_value = v->raw_value;
	v->raw_value = (can_1_buf[0]<<8)|can_1_buf[1];
	v->diff = v->raw_value - v->last_raw_value;
	if(v->diff <-7500)    //两次编码器的反馈值差别太大，表示圈数发生了改变
	{
			if( prepare_flag==0)
			v->round_cnt++;
		
			
	}
	else if(v->diff>7500)
	{
		
		if(prepare_flag==0)
			v->round_cnt--;
			
	
	}	
	
	if(v->raw_value-v->ecd_bias<-5000&&prepare_flag==1)
{
	 
		v->raw_value+=8192;  

}

	/*计算得到连续的编码器输出值*/
	v->ecd_value = v->raw_value + v->round_cnt * 8192;
	/*计算得到角度值，范围正负无穷大*/
	v->ecd_angle = (float)(v->raw_value - v->ecd_bias)*360/8192 + v->round_cnt * 360;
	/*计算转矩，单位未知，返回值*/
	v->Torque= (can_1_buf[2]<<8)|can_1_buf[3];

}


void EncoderProcessHIT(volatile Encoder *v)
{
	v->last_raw_value = v->raw_value;
	v->raw_value = (can_1_buf[0]<<8)|can_1_buf[1];
	v->diff = v->raw_value - v->last_raw_value;
	if(v->diff < -5500)    //两次编码器的反馈值差别太大，表示圈数发生了改变
	{
		v->round_cnt++;
		v->ecd_raw_rate = v->diff + 8192;
	}
	else if(v->diff>5500)
	{
		v->round_cnt--;
		v->ecd_raw_rate = v->diff- 8192;
	}		
	else
	{
		v->ecd_raw_rate = v->diff;
	}
	//计算得到连续的编码器输出值
	v->ecd_value = v->raw_value + v->round_cnt * 8192;
	//计算得到角度值，范围正负无穷大
	v->ecd_angle = (float)(v->raw_value - v->ecd_bias)*360/8192 + v->round_cnt * 360;		
}
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
